package dk.dtu.android.transport;

public class CalcDistance {
	// Constants
	private int a = 6378137; //major semi axis of earth in meters
	private int b = 6356752; // minor semi axis of earth in meters
	private double f = 1/298.257223563; // flattening (a-b)/a

	public double distVicenty(double srcLon, double srcLat, double dstLon, double dstLat) {
		final double L = Math.toRadians(dstLon - srcLon);
		final double U1 = Math.atan(1-f)*Math.tan(Math.toRadians(srcLat));
		final double U2 = Math.atan(1-f)*Math.tan(Math.toRadians(dstLat));
		final double sinU1 = Math.sin(U1);
		final double cosU1 = Math.cos(U1);
		final double sinU2 = Math.sin(U2);
		final double cosU2 = Math.cos(U2);
		
		double cosSqAlpha, sinSigma, cos2SigmaM, cosSigma, sigma;
		
		double lambda = L;
		double lambdaP = 0;
		int iterLim = 100;
		do {
			double sinLambda = Math.sin(lambda);
			double cosLambda = Math.cos(lambda);
			sinSigma = Math.sqrt((cosU2*sinLambda) * (cosU2*sinLambda) +
		      (cosU1*sinU2-sinU1*cosU2*cosLambda) * (cosU1*sinU2-sinU1*cosU2*cosLambda));
			if (sinSigma == 0) return 0;
			cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda;
			sigma = Math.atan2(sinSigma, cosSigma);
			double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
		    cosSqAlpha = 1 - sinAlpha*sinAlpha;
		    cos2SigmaM = cosSigma - 2*sinU1*sinU2/cosSqAlpha;
		    //if (isNaN(cos2SigmaM)) cos2SigmaM = 0;  // equatorial line: cosSqAlpha=0 (§6)
		    double C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha));
		    lambdaP = lambda;
		    lambda = L + (1-C) * f * sinAlpha *
		      (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));
		} while (Math.abs(lambda-lambdaP) > 1e-12 && --iterLim>0);
		
		if (iterLim == 0) return -1;
		
		double uSq = cosSqAlpha* (a*a - b*b)/(b*b);
		double A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
		double B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));
		double deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-
				B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));
		double s = b*A*(sigma-deltaSigma);
		  
		return s;
	}
}
